//
// Copyright (c) 2018 CNRS
//

#ifndef __pinocchio_lie_group_variant_visitor_hpp__
#define __pinocchio_lie_group_variant_visitor_hpp__

#include "pinocchio/multibody/liegroup/fwd.hpp"

namespace pinocchio
{

  /**
   * @brief      Visit a LieGroupVariant to get the dimension of
   *             the Lie group configuration space
   *
   * @param[in]  lg  the LieGroupVariant.
   *
   * @return     The dimension of the Lie group configuration space
   */
  template<typename LieGroupCollection>
  inline int nq(const LieGroupGenericTpl<LieGroupCollection> & lg);

  /**
   * @brief      Visit a LieGroupVariant to get the dimension of
   *             the Lie group tangent space
   *
   * @param[in]  lg  the LieGroupVariant.
   *
   * @return     The dimension of the Lie group tangent space
   */
  template<typename LieGroupCollection>
  inline int nv(const LieGroupGenericTpl<LieGroupCollection> & lg);

  /**
   * @brief      Visit a LieGroupVariant to get the name of it
   *
   * @param[in]  lg  the LieGroupVariant.
   *
   * @return     The Lie group name
   */
  template<typename LieGroupCollection>
  inline std::string name(const LieGroupGenericTpl<LieGroupCollection> & lg);

  /**
   * @brief      Visit a LieGroupVariant to get the neutral element of it
   *
   * @param[in]  lg  the LieGroupVariant.
   *
   * @return     The Lie group neutral element
   */
  template<typename LieGroupCollection>
  inline Eigen::
    Matrix<typename LieGroupCollection::Scalar, Eigen::Dynamic, 1, LieGroupCollection::Options>
    neutral(const LieGroupGenericTpl<LieGroupCollection> & lg);

  /**
   * @brief      Visit a LieGroupVariant to call its integrate method
   *
   * @param[in]  lg  the LieGroupVariant.
   * @param[in]  q   the starting configuration.
   * @param[in]  v   the tangent velocity.
   *
   */
  template<typename LieGroupCollection, class ConfigIn_t, class Tangent_t, class ConfigOut_t>
  inline void integrate(
    const LieGroupGenericTpl<LieGroupCollection> & lg,
    const Eigen::MatrixBase<ConfigIn_t> & q,
    const Eigen::MatrixBase<Tangent_t> & v,
    const Eigen::MatrixBase<ConfigOut_t> & qout);

  template<typename LieGroupCollection, class Config_t>
  inline void random(
    const LieGroupGenericTpl<LieGroupCollection> & lg, const Eigen::MatrixBase<Config_t> & qout);

  template<typename LieGroupCollection, class Config_t>
  inline void normalize(
    const LieGroupGenericTpl<LieGroupCollection> & lg, const Eigen::MatrixBase<Config_t> & qout);

  template<typename LieGroupCollection, class Config_t>
  inline bool isNormalized(
    const LieGroupGenericTpl<LieGroupCollection> & lg,
    const Eigen::MatrixBase<Config_t> & qin,
    const typename Config_t::Scalar & prec =
      Eigen::NumTraits<typename Config_t::Scalar>::dummy_precision());

  template<typename LieGroupCollection, class ConfigL_t, class ConfigR_t>
  inline bool isSameConfiguration(
    const LieGroupGenericTpl<LieGroupCollection> & lg,
    const Eigen::MatrixBase<ConfigL_t> & q0,
    const Eigen::MatrixBase<ConfigR_t> & q1,
    const typename ConfigL_t::Scalar & prec);

  template<typename LieGroupCollection, class ConfigL_t, class ConfigR_t>
  inline typename ConfigL_t::Scalar squaredDistance(
    const LieGroupGenericTpl<LieGroupCollection> & lg,
    const Eigen::MatrixBase<ConfigL_t> & q0,
    const Eigen::MatrixBase<ConfigR_t> & q1);

  template<typename LieGroupCollection, class ConfigL_t, class ConfigR_t>
  inline typename ConfigL_t::Scalar distance(
    const LieGroupGenericTpl<LieGroupCollection> & lg,
    const Eigen::MatrixBase<ConfigL_t> & q0,
    const Eigen::MatrixBase<ConfigR_t> & q1)
  {
    return std::sqrt(squaredDistance(lg, q0, q1));
  }

  template<typename LieGroupCollection, class ConfigL_t, class ConfigR_t, class Tangent_t>
  inline void difference(
    const LieGroupGenericTpl<LieGroupCollection> & lg,
    const Eigen::MatrixBase<ConfigL_t> & q0,
    const Eigen::MatrixBase<ConfigR_t> & q1,
    const Eigen::MatrixBase<Tangent_t> & v);

  template<typename LieGroupCollection, class ConfigL_t, class ConfigR_t, class ConfigOut_t>
  inline void randomConfiguration(
    const LieGroupGenericTpl<LieGroupCollection> & lg,
    const Eigen::MatrixBase<ConfigL_t> & q0,
    const Eigen::MatrixBase<ConfigR_t> & q1,
    const Eigen::MatrixBase<ConfigOut_t> & qout);

  template<typename LieGroupCollection, class ConfigL_t, class ConfigR_t, class ConfigOut_t>
  inline void interpolate(
    const LieGroupGenericTpl<LieGroupCollection> & lg,
    const Eigen::MatrixBase<ConfigL_t> & q0,
    const Eigen::MatrixBase<ConfigR_t> & q1,
    const typename ConfigL_t::Scalar & u,
    const Eigen::MatrixBase<ConfigOut_t> & qout);

  template<typename LieGroupCollection, class Config_t, class Tangent_t, class JacobianOut_t>
  void dIntegrate(
    const LieGroupGenericTpl<LieGroupCollection> & lg,
    const Eigen::MatrixBase<Config_t> & q,
    const Eigen::MatrixBase<Tangent_t> & v,
    const Eigen::MatrixBase<JacobianOut_t> & J,
    const ArgumentPosition arg,
    const AssignmentOperatorType op = SETTO);

  template<
    typename LieGroupCollection,
    class Config_t,
    class Tangent_t,
    class JacobianIn_t,
    class JacobianOut_t>
  void dIntegrate(
    const LieGroupGenericTpl<LieGroupCollection> & lg,
    const Eigen::MatrixBase<Config_t> & q,
    const Eigen::MatrixBase<Tangent_t> & v,
    const Eigen::MatrixBase<JacobianIn_t> & J_in,
    int self,
    const Eigen::MatrixBase<JacobianOut_t> & J_out,
    const ArgumentPosition arg,
    const AssignmentOperatorType op = SETTO);

  template<
    typename LieGroupCollection,
    class Config_t,
    class Tangent_t,
    class JacobianIn_t,
    class JacobianOut_t>
  void dIntegrate(
    const LieGroupGenericTpl<LieGroupCollection> & lg,
    const Eigen::MatrixBase<Config_t> & q,
    const Eigen::MatrixBase<Tangent_t> & v,
    int self,
    const Eigen::MatrixBase<JacobianIn_t> & J_in,
    const Eigen::MatrixBase<JacobianOut_t> & J_out,
    const ArgumentPosition arg,
    const AssignmentOperatorType op = SETTO);

  template<typename LieGroupCollection, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
  void dDifference(
    const LieGroupGenericTpl<LieGroupCollection> & lg,
    const Eigen::MatrixBase<ConfigL_t> & q0,
    const Eigen::MatrixBase<ConfigR_t> & q1,
    const Eigen::MatrixBase<JacobianOut_t> & J,
    const ArgumentPosition arg);

  template<
    typename LieGroupCollection,
    class ConfigL_t,
    class ConfigR_t,
    class JacobianIn_t,
    class JacobianOut_t>
  void dDifference(
    const LieGroupGenericTpl<LieGroupCollection> & lg,
    const Eigen::MatrixBase<ConfigL_t> & q0,
    const Eigen::MatrixBase<ConfigR_t> & q1,
    const Eigen::MatrixBase<JacobianIn_t> & Jin,
    int self,
    const Eigen::MatrixBase<JacobianOut_t> & Jout,
    const ArgumentPosition arg);

  template<
    typename LieGroupCollection,
    class ConfigL_t,
    class ConfigR_t,
    class JacobianIn_t,
    class JacobianOut_t>
  void dDifference(
    const LieGroupGenericTpl<LieGroupCollection> & lg,
    const Eigen::MatrixBase<ConfigL_t> & q0,
    const Eigen::MatrixBase<ConfigR_t> & q1,
    int self,
    const Eigen::MatrixBase<JacobianIn_t> & Jin,
    const Eigen::MatrixBase<JacobianOut_t> & Jout,
    const ArgumentPosition arg);

  template<
    typename LieGroupCollection,
    class Config_t,
    class Tangent_t,
    class JacobianIn_t,
    class JacobianOut_t>
  void dIntegrateTransport(
    const LieGroupGenericTpl<LieGroupCollection> & lg,
    const Eigen::MatrixBase<Config_t> & q,
    const Eigen::MatrixBase<Tangent_t> & v,
    const Eigen::MatrixBase<JacobianIn_t> & J_in,
    const Eigen::MatrixBase<JacobianOut_t> & J_out,
    const ArgumentPosition arg);

  template<typename LieGroupCollection, class Config_t, class Tangent_t, class JacobianOut_t>
  void dIntegrateTransport(
    const LieGroupGenericTpl<LieGroupCollection> & lg,
    const Eigen::MatrixBase<Config_t> & q,
    const Eigen::MatrixBase<Tangent_t> & v,
    const Eigen::MatrixBase<JacobianOut_t> & J,
    const ArgumentPosition arg);
} // namespace pinocchio

/// Details
#include "pinocchio/multibody/liegroup/liegroup-variant-visitors.hxx"

#endif // ifndef __pinocchio_lie_group_variant_visitor_hpp__
